# ===============================================================#
#                          平滑速度衰减函数
# ===============================================================#

import math

def obs_vfrict(robot, xmin, ymin, dL, flag):
    robot.Ox = xmin * dL
    robot.Oy = ymin * dL
    robot.vo = robot.r0_ob_v
    robot.vo_k = math.atan2(ymin-robot.y, xmin-robot.x)
    if flag == 1:      # 仅粘性
        dx = robot.v_ob_frict() * math.cos(robot.v_frict_th())
        dy = robot.v_ob_frict() * math.sin(robot.v_frict_th())
    else:
        if flag == 2:  # 仅排斥
            dx = robot.v_ob_rep * math.cos(robot.th())
            dy = robot.v_ob_rep * math.sin(robot.th())
        else:
            if flag == 3:  # 粘性+排斥
                dx = robot.v_ob_rep * math.cos(robot.th()) + robot.v_ob_frict() * math.cos(robot.v_frict_th())
                dy = robot.v_ob_rep * math.sin(robot.th()) + robot.v_ob_frict() * math.sin(robot.v_frict_th())
    return dx, dy



